# -*- coding: utf-8 -*-
from copy import deepcopy

import numpy as np
from naoqi import qi
import cv2

class Hough():
    def __init__(self):
        self.ses = qi.Session()
        self.ses.connect("tcp://192.168.61.115:9559")
        self.video = self.ses.service("ALVideoDevice")
        self.post=self.ses.service("ALRobotPosture")
        self.motion=self.ses.service("ALMotion")
        self.post.goToPosture("Crouch", 0.5)
        self.motion.setAngles(["HeadYaw","HeadPitch"],[0,np.pi/7],0.2)
        self.subscriber = self.video.subscribeCamera("demo", 0, 2, 13, 60)
hough=Hough()
try:
    while True:
        imageNAO = hough.video.getImageRemote(hough.subscriber)

        if imageNAO == None:
            print('cannot capture.')
        elif imageNAO[6] == None:
            print('no image data string.')
        else:
            frameArray = np.frombuffer(imageNAO[6], dtype=np.uint8).reshape(
                [imageNAO[1], imageNAO[0], imageNAO[2]])
            image_gray = cv2.cvtColor(frameArray, cv2.COLOR_BGR2GRAY)
            retval,image_thre=cv2.threshold(image_gray,200,255, cv2.THRESH_BINARY)
            result=deepcopy(image_gray)
            contours=cv2.Canny(image_thre,190,300)

            cv2.imshow("pepper-top-camera-asdf", contours)

            lines = cv2.HoughLinesP(contours, 1, np.pi / 180, 150)
            if lines is not None:
                for line in lines:
                    x1, y1, x2, y2 = line[0]
                    cv2.line(result, (x1, y1), (x2, y2), (0, 255, 0), 2)
            cv2.imshow("pepper-top-camera-1280x800", result)
            print lines
            print np.shape(lines)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            hough.video.releaseImage(hough.subscriber)
            hough.video.unsubscribe(hough.subscriber)
            break
except :
    print
    hough.video.releaseImage(hough.subscriber)
    hough.video.unsubscribe(hough.subscriber)